#include "quaternion.h"

/************************************
/*          Public
/************************************/

    /************************************/
    /*          Constructor
    /************************************/

        Quaternion::Quaternion(){
            a=b=c=0;
            d=1;
        }

        Quaternion::Quaternion(float aa, float bb, float cc, float angle){
            float sin_a = sin(angle / 2.0);
            float cos_a = cos(angle / 2.0);

            a = aa * sin_a;
            b = bb * sin_a;
            c = cc * sin_a;
            d = cos_a;

            Normalize();
        }

    /************************************
    /*          Methods
    /************************************/

        void Quaternion::Normalize()
        {
            float magnitude = Magnitude();

            if(magnitude>0.0)
            {
                a   =   a/magnitude;
                b   =   b/magnitude;
                c   =   c/magnitude;
                d   =   d/magnitude;
            }
        }

float * Quaternion::RotationMatrix()
{
    float * matriceRotation = new float[16];

    float xx      =  a * a;
    float xy      =  a * b;
    float xz      =  a * c;
    float xw      =  a * d;

    float yy      = b * b;
    float yz      = b * c;
    float yw      = b * d;

    float zz      = c * c;
    float zw      = c * d;

    matriceRotation[0]  = 1 - 2 * ( yy + zz );
    matriceRotation[1]  =     2 * ( xy - zw );
    matriceRotation[2]  =     2 * ( xz + yw );

    matriceRotation[4]  =     2 * ( xy + zw );
    matriceRotation[5]  = 1 - 2 * ( xx + zz );
    matriceRotation[6]  =     2 * ( yz - xw );

    matriceRotation[8]  =     2 * ( xz - yw );
    matriceRotation[9]  =     2 * ( yz + xw );
    matriceRotation[10] = 1 - 2 * ( xx + yy );

    matriceRotation[3]  = matriceRotation[7] = matriceRotation[11] = matriceRotation[12] = matriceRotation[13] = matriceRotation[14] = 0;
    matriceRotation[15] = 1;

    return matriceRotation;

}

float Quaternion::Magnitude()
{
    Quaternion conj = Conjuge();

    return sqrt(a*a + b*b + c*c + d*d);

}

Quaternion & Quaternion::operator *(const Quaternion & quat)
{
    Quaternion * qres = new Quaternion();

        qres->a = d*quat.a + a*quat.d + b*quat.c - c*quat.b;
        qres->b = d*quat.b + b*quat.d + c*quat.a - a*quat.c;
        qres->c = d*quat.c + c*quat.d + a*quat.b - b*quat.a;
        qres->d = d*quat.d - a*quat.a - b*quat.b - c*quat.c;

    return *qres;
}

Quaternion & Quaternion::Conjuge()
{
    Quaternion * qres   =   new Quaternion();

        qres->a = -a;
        qres->b = -b;
        qres->c = -c;
        qres->d = d;

    return * qres;
}


void Quaternion::Print()
{
    printf("%f, %f, %f, %f \n",a, b, c, d);
}

    /************************************/
    /*          Static                  */
    /************************************/

        /************************************/
        /*          Members                 */
        /************************************/

            Quaternion & Quaternion::Euler(float x, float y, float z)
            {
                Quaternion * quatx  =   new Quaternion(1.0, 0.0, 0.0, x);
                Quaternion *quaty=   new Quaternion(0.0, 1.0, 0.0, y);
                Quaternion *quatz=   new Quaternion(0.0, 0.0, 1.0, z);

                return (*quatx)*(*quaty)*(*quatz);
            }
